
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_bmi270.c
  * @author     baiyang
  * @date       2023-2-7
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_imu_bmi270.h"

#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/
//#define BMI270_DEBUG

#define BMI270_CHIP_ID 0x24
#define BMI270_CMD_SOFTRESET 0xB6
#define BMI270_CMD_FIFOFLUSH 0xB0
#define BMI270_FIFO_ACC_EN 0x40
#define BMI270_FIFO_GYR_EN 0x80
#define BMI270_BACKEND_SAMPLE_RATE 1000

/* Datasheet says that the device powers up and soft resets in 2s, so waiting for
 * 5ms before initialization is enough. */
#define BMI270_POWERUP_DELAY_MSEC 5

/* This number of samples should provide only one read burst operation on the
 * FIFO most of the time (99.99%). */
#define BMI270_MAX_FIFO_SAMPLES 8

#define BMI270_HARDWARE_INIT_MAX_TRIES 5
/*----------------------------------typedef-----------------------------------*/
// BMI270 registers (not the complete list)
enum BMI270Register {
    BMI270_REG_CHIP_ID = 0x00,
    BMI270_REG_ERR_REG = 0x02,
    BMI270_REG_STATUS = 0x03,
    BMI270_REG_ACC_DATA_X_LSB = 0x0C,
    BMI270_REG_GYR_DATA_X_LSB = 0x12,
    BMI270_REG_SENSORTIME_0 = 0x18,
    BMI270_REG_SENSORTIME_1 = 0x19,
    BMI270_REG_SENSORTIME_2 = 0x1A,
    BMI270_REG_EVENT = 0x1B,
    BMI270_REG_INT_STATUS_0 = 0x1C,
    BMI270_REG_INT_STATUS_1 = 0x1D,
    BMI270_REG_INTERNAL_STATUS = 0x21,
    BMI270_REG_TEMPERATURE_LSB = 0x22,
    BMI270_REG_TEMPERATURE_MSB = 0x23,
    BMI270_REG_FIFO_LENGTH_LSB = 0x24,
    BMI270_REG_FIFO_LENGTH_MSB = 0x25,
    BMI270_REG_FIFO_DATA = 0x26,
    BMI270_REG_ACC_CONF = 0x40,
    BMI270_REG_ACC_RANGE = 0x41,
    BMI270_REG_GYRO_CONF = 0x42,
    BMI270_REG_GYRO_RANGE = 0x43,
    BMI270_REG_AUX_CONF = 0x44,
    BMI270_REG_FIFO_DOWNS = 0x45,
    BMI270_REG_FIFO_WTM_0 = 0x46,
    BMI270_REG_FIFO_WTM_1 = 0x47,
    BMI270_REG_FIFO_CONFIG_0 = 0x48,
    BMI270_REG_FIFO_CONFIG_1 = 0x49,
    BMI270_REG_SATURATION = 0x4A,
    BMI270_REG_INT1_IO_CTRL = 0x53,
    BMI270_REG_INT2_IO_CTRL = 0x54,
    BMI270_REG_INT_LATCH = 0x55,
    BMI270_REG_INT1_MAP_FEAT = 0x56,
    BMI270_REG_INT2_MAP_FEAT = 0x57,
    BMI270_REG_INT_MAP_DATA = 0x58,
    BMI270_REG_INIT_CTRL = 0x59,
    BMI270_REG_INIT_DATA = 0x5E,
    BMI270_REG_ACC_SELF_TEST = 0x6D,
    BMI270_REG_GYR_SELF_TEST_AXES = 0x6E,
    BMI270_REG_PWR_CONF = 0x7C,
    BMI270_REG_PWR_CTRL = 0x7D,
    BMI270_REG_CMD = 0x7E,
};

/*---------------------------------prototype----------------------------------*/
static void sensor_imu_bmi270_start(sensor_imu_backend *backend);
static bool sensor_imu_bmi270_update(sensor_imu_backend *backend);
static bool init(sensor_imu_bmi270_t bmi270);
static bool hardware_init(sensor_imu_bmi270_t bmi270);
static bool read_registers(sensor_imu_bmi270_t bmi270, uint8_t reg, uint8_t *data, uint8_t len);
static bool write_register(sensor_imu_bmi270_t bmi270, uint8_t reg, uint8_t v);
static void configure_accel(sensor_imu_bmi270_t bmi270);
static void configure_gyro(sensor_imu_bmi270_t bmi270);
static void configure_fifo(sensor_imu_bmi270_t bmi270);
static void fifo_reset(sensor_imu_bmi270_t bmi270);
static void check_err_reg(sensor_imu_bmi270_t bmi270);
static void read_fifo(void *parameter);
static void parse_accel_frame(sensor_imu_bmi270_t bmi270, const uint8_t* d);
static void parse_gyro_frame(sensor_imu_bmi270_t bmi270, const uint8_t* d);
/*----------------------------------variable----------------------------------*/
static const uint8_t maximum_fifo_config_file[] = { BMI270_REG_INIT_DATA,
    0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00,
    0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5,
    0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22,
    0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1,
    0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00,
    0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb,
    0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5,
    0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41,
    0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24,
    0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94,
    0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f,
    0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20,
    0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08,
    0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0,
    0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84,
    0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61,
    0xf5, 0xeb, 0x2c, 0xe1, 0x6f
};

static const uint32_t BACKEND_PERIOD_US = 1000000UL / BMI270_BACKEND_SAMPLE_RATE;

static struct sensor_imu_backend_ops bmi270_ops = {.sensor_imu_backend_destructor = NULL,
                                                   .update = sensor_imu_bmi270_update,
                                                   .accumulate = NULL,
                                                   .start = sensor_imu_bmi270_start,
                                                   .get_output_banner = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_imu_bmi270_ctor(sensor_imu_bmi270_t bmi270,
                                    gp_device_t dev,
                                    enum RotationEnum rotation)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(bmi270, 0, sizeof(struct sensor_imu_bmi270));

    sensor_imu_backend_ctor(&bmi270->backend, &bmi270_ops, "bmi270");

    bmi270->_rotation = rotation;
    
    bmi270->_dev = dev;
}

static void sensor_imu_bmi270_start(sensor_imu_backend *backend)
{
    sensor_imu_bmi270_t bmi270 = (sensor_imu_bmi270_t)backend;

    configure_accel(bmi270);

    configure_gyro(bmi270);

    configure_fifo(bmi270);

    if (!sensor_imu_register_accel(&bmi270->_accel_instance, BMI270_BACKEND_SAMPLE_RATE, devmgr_get_bus_id_devtype(bmi270->_dev, DEVTYPE_BMI270)) ||
        !sensor_imu_register_gyro(&bmi270->_gyro_instance, BMI270_BACKEND_SAMPLE_RATE, devmgr_get_bus_id_devtype(bmi270->_dev, DEVTYPE_BMI270))) {
        return;
    }

    // setup sensor rotations from probe()
    sensor_imu_backend_set_gyro_orientation(bmi270->_gyro_instance, bmi270->_rotation);
    sensor_imu_backend_set_accel_orientation(bmi270->_accel_instance, bmi270->_rotation);

    /* Call read_fifo() at 1600Hz */
    bmi270->periodic_handle = devmgr_register_periodic_callback(bmi270->_dev, BACKEND_PERIOD_US, read_fifo, bmi270);
}

static bool sensor_imu_bmi270_update(sensor_imu_backend *backend)
{
    sensor_imu_backend_update_accel(backend, ((sensor_imu_bmi270_t)backend)->_accel_instance);
    sensor_imu_backend_update_gyro(backend, ((sensor_imu_bmi270_t)backend)->_gyro_instance);
    return true;
}

sensor_imu_backend* sensor_imu_bmi270_probe(gp_device_t dev, enum RotationEnum rotation)
{
    if (!dev) {
        return NULL;
    }

    sensor_imu_bmi270_t sensor = (sensor_imu_bmi270_t)rt_malloc(sizeof(struct sensor_imu_bmi270));

    if (!sensor) {
        return NULL;
    }

    sensor_imu_bmi270_ctor(sensor, dev, rotation);

    if (!init(sensor)) {
        sensor_imu_backend_destructor(&sensor->backend);
        rt_free(sensor);
        return NULL;
    }

    return (sensor_imu_backend *)sensor;
}

static bool init(sensor_imu_bmi270_t bmi270)
{
    devmgr_set_read_flag(bmi270->_dev, 0x80);

    return hardware_init(bmi270);
}

static bool hardware_init(sensor_imu_bmi270_t bmi270)
{
    bool init = false;
    bool read_chip_id = false;

    rt_thread_mdelay(BMI270_POWERUP_DELAY_MSEC);

    devmgr_set_speed(bmi270->_dev, DEV_SPEED_LOW);

    for (unsigned i = 0; i < BMI270_HARDWARE_INIT_MAX_TRIES; i++) {

        uint8_t chip_id = 0;
        /* If CSB sees a rising edge after power-up, the device interface switches to SPI
         * after 200 μs until a reset or the next power-up occurs therefore it is recommended
         * to perform a SPI single read of register CHIP_ID (the obtained value will be invalid)
         * before the actual communication start, in order to use the SPI interface */
        read_registers(bmi270, BMI270_REG_CHIP_ID, &chip_id, 1);
        rt_thread_mdelay(1);

        write_register(bmi270, BMI270_REG_CMD, BMI270_CMD_SOFTRESET);
        rt_thread_mdelay(BMI270_POWERUP_DELAY_MSEC);    // power on and soft reset time is 2ms

        // switch to SPI mode again
        read_registers(bmi270, BMI270_REG_CHIP_ID, &chip_id, 1);
        rt_thread_mdelay(1);

        read_registers(bmi270, BMI270_REG_CHIP_ID, &chip_id, 1);
        if (chip_id != BMI270_CHIP_ID) {
            continue;
        }

        // successfully identified the chip, proceed with initialisation
        read_chip_id = true;

        // disable power save
        write_register(bmi270, BMI270_REG_PWR_CONF, 0x00);
        rt_thread_mdelay(1); // needs to be at least 450us

        // upload config
        write_register(bmi270, BMI270_REG_INIT_CTRL, 0x00);

        // Transfer the config file, data packet includes INIT_DATA
        devmgr_transfer(bmi270->_dev, maximum_fifo_config_file, sizeof(maximum_fifo_config_file), NULL, 0);

        // config is done
        write_register(bmi270, BMI270_REG_INIT_CTRL, 1);

        // check the results
        rt_thread_mdelay(20);

        uint8_t status = 0;
        read_registers(bmi270, BMI270_REG_INTERNAL_STATUS, &status, 1);

        if ((status & 1) == 1) {
            init = true;
            DEV_PRINTF("BMI270 initialized after %d retries\n", i+1);
            break;
        }
    }

    devmgr_set_speed(bmi270->_dev, DEV_SPEED_HIGH);

    if (read_chip_id && !init) {
        DEV_PRINTF("BMI270: failed to init\n");
    }

    return init;
}

/*
  read from registers, special SPI handling needed
*/
static bool read_registers(sensor_imu_bmi270_t bmi270, uint8_t reg, uint8_t *data, uint8_t len)
{
    // for SPI we need to discard the first returned byte. See
    // datasheet for explanation
    uint8_t b[len+2];
    b[0] = reg | 0x80;
    memset(&b[1], 0, len+1);
    if (!devmgr_transfer(bmi270->_dev, b, len+2, b, len+2)) {
        return false;
    }
    memcpy(data, &b[2], len);
    return true;
}

/*
  write registers with retries. The SPI sensor may take
  several tries to correctly write a register
*/
static bool write_register(sensor_imu_bmi270_t bmi270, uint8_t reg, uint8_t v)
{
    for (uint8_t i=0; i<8; i++) {
        devmgr_write_register(bmi270->_dev, reg, v, false);
        uint8_t v2 = 0;
        if (read_registers(bmi270, reg, &v2, 1) && v2 == v) {
            return true;
        }
    }
    return false;
}

static void configure_accel(sensor_imu_bmi270_t bmi270)
{
    // set acc in high performance mode with OSR4 filtering (751Hz/4) at 1600Hz
    // see https://community.bosch-sensortec.com/t5/MEMS-sensors-forum/BMI270-OSR-mode-behaviour/td-p/52020
    // OSR4 is a 188Hz filter cutoff, acc_bwp == 0, equivalent to other driver filters
    write_register(bmi270, BMI270_REG_ACC_CONF, 1U<<7 | 0x0C);
    // set acc to 16G full scale
    write_register(bmi270, BMI270_REG_ACC_RANGE, 0x03);

    check_err_reg(bmi270);
}

static void configure_gyro(sensor_imu_bmi270_t bmi270)
{
    // set gyro in high performance filter mode, high performance noise mode, normal filtering at 3.2KHz
    // filter cutoff 751hz
    write_register(bmi270, BMI270_REG_GYRO_CONF, 1U<<7 | 1<<6 | 2<<4 | 0x0D);
    // set gyro to 2000dps full scale
    // for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well
    // or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!)
    write_register(bmi270, BMI270_REG_GYRO_RANGE, 0x08);

    check_err_reg(bmi270);
}

static void configure_fifo(sensor_imu_bmi270_t bmi270)
{
    // stop when full, disable sensortime frame
    write_register(bmi270, BMI270_REG_FIFO_CONFIG_0, 0x01);
    // accel + gyro data in FIFO together with headers
    write_register(bmi270, BMI270_REG_FIFO_CONFIG_1, 1U<<7 | 1U<<6 | 1U<<4);
    // filtered data downsampled by 2**1 to 1600Hz
    write_register(bmi270, BMI270_REG_FIFO_DOWNS, 1U<<7 | 1U<<3 | 0x01);
    // disable advanced power save, enable FIFO self-wake
    write_register(bmi270, BMI270_REG_PWR_CONF, 0x02);
    // Enable the gyro, accelerometer and temperature sensor - disable aux interface
    write_register(bmi270, BMI270_REG_PWR_CTRL, 0x0E);

    fifo_reset(bmi270);

    check_err_reg(bmi270);
}

static void fifo_reset(sensor_imu_bmi270_t bmi270)
{
    // flush and reset FIFO
    write_register(bmi270, BMI270_REG_CMD, BMI270_CMD_FIFOFLUSH);

    sensor_imu_backend_notify_accel_fifo_reset(bmi270->_accel_instance);
    sensor_imu_backend_notify_gyro_fifo_reset(bmi270->_gyro_instance);
}

static void check_err_reg(sensor_imu_bmi270_t bmi270)
{
#if defined(BMI270_DEBUG)
    uint8_t err = 0;
    read_registers(bmi270, BMI270_REG_ERR_REG, &err, 1);

    if (err) {
        if ((err & 1) == 1) {
            uint8_t status =  0;
            read_registers(bmi270, BMI270_REG_INTERNAL_STATUS, &status, 1);
            switch (status & 0xF) {
            case 0:
                console_panic("BMI270: not_init");
                break;
            case 2:
                console_panic("BMI270: init_err");
                break;
            case 3:
                console_panic("BMI270: drv_err");
                break;
            case 4:
                console_panic("BMI270: sns_stop");
                break;
            case 5:
                console_panic("BMI270: nvm_error");
                break;
            case 6:
                console_panic("BMI270: start_up_error");
                break;
            case 7:
                console_panic("BMI270: compat_error");
                break;
            case 1: // init ok
                if ((status>>5 & 1) == 1) {
                    console_panic("BMI270: axes_remap_error");
                } else if ((status>>6 & 1) == 1) {
                    console_panic("BMI270: odr_50hz_error");
                }
                break;
            }
        } else if ((err>>6 & 1) == 1) {
            console_panic("BMI270: fifo_err");
        } else if ((err>>7 & 1) == 1) {
            console_panic("BMI270: aux_err");
        } else {
            console_panic("BMI270: internal error detected %d", err>>1 & 0xF);
        }
    }
#endif
}

/*
  read fifo
 */
static void read_fifo(void *parameter)
{
    sensor_imu_bmi270_t bmi270 = (sensor_imu_bmi270_t)parameter;

    // check for FIFO errors/overflow
    uint8_t err = 0;
    read_registers(bmi270, BMI270_REG_ERR_REG, &err, 1);
    if ((err>>6 & 1) == 1) {
        fifo_reset(bmi270);
        return;
    }

    uint8_t len[2];
    if (!read_registers(bmi270, BMI270_REG_FIFO_LENGTH_LSB, len, 2)) {
        sensor_imu_backend_inc_accel_error_count(bmi270->_accel_instance);
        sensor_imu_backend_inc_gyro_error_count(bmi270->_gyro_instance);
        return;
    }
    uint16_t fifo_length = len[0] + (len[1]<<8);
    if (fifo_length & 0x8000) {
        // empty
        return;
    }

    // don't read more than 8 frames at a time
    if (fifo_length > BMI270_MAX_FIFO_SAMPLES*13) {
        fifo_length = BMI270_MAX_FIFO_SAMPLES*13;
    }
    if (fifo_length == 0) {
        return;
    }

    uint8_t data[fifo_length];
    if (!read_registers(bmi270, BMI270_REG_FIFO_DATA, data, fifo_length)) {
        sensor_imu_backend_inc_accel_error_count(bmi270->_accel_instance);
        sensor_imu_backend_inc_gyro_error_count(bmi270->_gyro_instance);
        return;
    }

    // adjust the periodic callback to be synchronous with the incoming data
    // this means that we rarely run read_fifo() without updating the sensor data
    devmgr_adjust_periodic_callback(bmi270->_dev, bmi270->periodic_handle, BACKEND_PERIOD_US);

    const uint8_t *p = &data[0];
    while (fifo_length >= 12) {
        /*
          the fifo frames are variable length, with the frame type in the first byte
         */
        uint8_t frame_len = 2;
        switch (p[0] & 0xFC) {
        case 0x84: // accel
            frame_len = 7;
            parse_accel_frame(bmi270, p+1);
            break;
        case 0x88: // gyro
            frame_len = 7;
            parse_gyro_frame(bmi270, p+1);
            break;
        case 0x8C: // accel + gyro
            frame_len = 13;
            parse_gyro_frame(bmi270, p+1);
            parse_accel_frame(bmi270, p+7);
            break;
        case 0x40:
            // skip frame
            frame_len = 2;
            break;
        case 0x44:
            // sensortime frame
            frame_len = 4;
            break;
        case 0x48:
            // fifo config frame
            frame_len = 5;
            break;
        case 0x50:
            // sample drop frame
            frame_len = 2;
            break;
        case 0x80:
            // invalid frame
            fifo_reset(bmi270);
            return;
        }
        p += frame_len;
        fifo_length -= frame_len;
    }

    // temperature sensor updated every 10ms
    if (bmi270->temperature_counter++ == 100) {
        bmi270->temperature_counter = 0;
        uint8_t tbuf[2];
        if (!read_registers(bmi270, BMI270_REG_TEMPERATURE_LSB, tbuf, 2)) {
            sensor_imu_backend_inc_accel_error_count(bmi270->_accel_instance);
            sensor_imu_backend_inc_gyro_error_count(bmi270->_gyro_instance);
        } else {
            uint16_t tval = tbuf[0] | (tbuf[1] << 8);
            if (tval != 0x8000) {   // 0x8000 is invalid
                int16_t klsb = (int16_t)(tval);
                float temp_degc = klsb * 0.002f + 23.0f;
                sensor_imu_backend_publish_temperature(&bmi270->backend, bmi270->_accel_instance, temp_degc);
            }
        }
    }
}

static void parse_accel_frame(sensor_imu_bmi270_t bmi270, const uint8_t* d)
{
    // assume configured for 16g range
    const float scale = (1.0/32768.0) * GRAVITY_MSS * 16.0f;
    int16_t xyz[3] = {
        (int16_t)((uint16_t)(d[0] | (d[1]<<8))),
        (int16_t)((uint16_t)(d[2] | (d[3]<<8))),
        (int16_t)((uint16_t)(d[4] | (d[5]<<8)))};
    Vector3f_t accel = {xyz[0], xyz[1], xyz[2]};

    accel.x *= scale;
    accel.y *= scale;
    accel.z *= scale;

    sensor_imu_backend_rotate_and_correct_accel(&bmi270->backend, bmi270->_accel_instance, &accel);
    sensor_imu_backend_notify_new_accel_raw_sample(&bmi270->backend, bmi270->_accel_instance, &accel, 0, false);
}

static void parse_gyro_frame(sensor_imu_bmi270_t bmi270, const uint8_t* d)
{
    // data is 16 bits with 2000dps range
    const float scale = radians(2000.0f) / 32767.0f;
    int16_t xyz[3] = {
        (int16_t)((uint16_t)(d[0] | d[1]<<8)),
        (int16_t)((uint16_t)(d[2] | d[3]<<8)),
        (int16_t)((uint16_t)(d[4] | d[5]<<8))};
    Vector3f_t gyro = {xyz[0], xyz[1], xyz[2]};

    gyro.x *= scale;
    gyro.y *= scale;
    gyro.z *= scale;

    sensor_imu_backend_rotate_and_correct_gyro(&bmi270->backend, bmi270->_gyro_instance, &gyro);
    sensor_imu_backend_notify_new_gyro_raw_sample(&bmi270->backend, bmi270->_gyro_instance, &gyro, 0);
}

/*------------------------------------test------------------------------------*/


